//
// Created by javier on 6/11/15.
//

#include "simple_rinex2_writer.h"
#include <cmath>
#include <utility>


bool simple_rinex2_writer::write_obs_header(const std::string &fecha_rinex_str, std::vector<double> rx_pos, float interv, datetime date_time)
{
    std::string caux1, caux2, caux3, caux4, caux5, caux6, coment, marker, caux40;
    float vers = 2.1;
    caux1 = "OBSERVATION DATA";
    caux2 = "G (GPS)";
    caux3 = "RINEX VERSION / TYPE";
    fprintf(obs_file_ptr, "%#9.3g%-11s%-20s%-20s%-20s\n", vers, " ", caux1.c_str(), caux2.c_str(), caux3.c_str());

    caux1 = "SiGOGbcst v.1       ";
    caux2 = "                    ";
    caux3 = "PGM / RUN BY / DATE ";
    fprintf(obs_file_ptr, "%-20s%-20s%-20s%-20s\n", caux1.c_str(), caux2.c_str(), fecha_rinex_str.c_str(), caux3.c_str());

    coment = "SIMULATED RINEX from BROADCASTED ORBITS";
    caux1 = "COMMENT";
    fprintf(obs_file_ptr, "%-60s%-20s\n", coment.c_str(), caux1.c_str());

    marker = "sim-marker";
    caux1 = "MARKER NAME";
    fprintf(obs_file_ptr, "%-60s%-20s\n", marker.c_str(), caux1.c_str());

    caux1 = "TIMON User";
    caux40 = "CTTC";
    caux2 = "OBSERVER / AGENCY";
    fprintf(obs_file_ptr, "%-20s%-40s%-20s\n", caux1.c_str(), caux40.c_str(), caux2.c_str());

    caux1 = "X124A123";
    caux2 = "XX";
    caux3 = "ZZZ";
    caux4 = "REC # / TYPE / VERS";
    fprintf(obs_file_ptr, "%-20s%-20s%-20s%-20s\n", caux1.c_str(), caux2.c_str(), caux3.c_str(), caux4.c_str());

    caux1 = "234";
    caux2 = "YY";
    caux3 = " ";
    caux4 = "ANT # / TYPE";
    fprintf(obs_file_ptr, "%-20s%-20s%-20s%-20s\n", caux1.c_str(), caux2.c_str(), caux3.c_str(), caux4.c_str());

    caux1 = "APPROX POSITION XYZ";
    fprintf(obs_file_ptr, "%#14.4f%#14.4f%14.4f%-18s%-20s\n", rx_pos[0], rx_pos[1], rx_pos[2], " ", caux1.c_str());

    caux1 = "ANTENNA: DELTA H/E/N";
    fprintf(obs_file_ptr, "%#14.4f%#14.4f%#14.4f%-18s%-20s\n", 0.0, 0.0, 0.0, " ", caux1.c_str());

    int wvfaL1 = 1;
    int wvfaL2 = 1;
    caux1 = "WAVELENGTH FACT L1/2";
    fprintf(obs_file_ptr, "%6d%6d%-48s%-20s\n", wvfaL1, wvfaL2, " ", caux1.c_str());

    fprintf(obs_file_ptr, "     5    C1    P1    P2    L1    D1                        # / TYPES OF OBSERV \n");

    caux1 = "INTERVAL";
    fprintf(obs_file_ptr, "%#10.3f%-50s%-20s\n", interv, " ", caux1.c_str());


    std::string tsys = "GPS";
    caux1 = "TIME OF FIRST OBS";
    fprintf(obs_file_ptr, "%6d%6d%6d%6d%6d%#13.7f%-5s%-3s%-9s%-20s\n", date_time.y, date_time.m, date_time.d, date_time.hh, date_time.mm, date_time.sec, " ", tsys.c_str(), " ", caux1.c_str());


    caux1 = "END OF HEADER";
    fprintf(obs_file_ptr, "%-60s%-20s\n", " ", caux1.c_str());

    return true;
}

bool simple_rinex2_writer::write_obs_epoch(int nsats, int *prn, gpstime *gps_time, const double *dist_m, const double *acc_carrier_phase_cycles, double *acc_carrier_phase_l2_cycles, const double *doppler_l1)
{
    if (nsats > 0)
        {
            //limit nsats = 12 for RINEX file
            if (nsats > 12)
                {
                    nsats = 12;
                }

            datetime t;
            t.gps2date(gps_time);
            int obs_year2 = floor(fmod(t.y, 100.0));

            fprintf(obs_file_ptr, "%-1s%02d%-1s%2d%-1s%2d%-1s%2d%-1s%2d%#11.7f%-2s%1d%3d", " ", obs_year2, " ", t.m, " ", t.d, " ", t.hh, " ", t.mm, t.sec, " ", 0, nsats);

            for (int n = 0; n < nsats; n++)
                {
                    fprintf(obs_file_ptr, "%3d", prn[n]);
                }

            if (nsats < 12)
                {
                    for (int n = nsats; n < 12; n++)
                        {
                            fprintf(obs_file_ptr, "%-3s", " ");
                        }
                }
            fprintf(obs_file_ptr, "\n");

            double c1, p1, p2, l1, l2, d1;
            double c = 299792458.0;
            double f_L1 = 1575.42e6;
            double f_L2 = 1227.60e6;

            for (int n = 0; n < nsats; n++)
                {
                    //%C Build observables:
                    //% C1 (GPS C/A L1 pseudorange
                    c1 = dist_m[n];
                    //% P1 (GPS P L1 pseudorange)
                    p1 = dist_m[n];
                    //% P2 (GPS P L2 pseudorange)
                    p2 = dist_m[n];
                    //% L1 (GPS L1 carrier phase)
                    //l1=(dist_m[n]/c)*f_L1;
                    l1 = acc_carrier_phase_cycles[n];
                    //% L2 (GPS L2 carrier phase)
                    //l2=(dist_m[n]/c)*f_L2;
                    //l2=acc_carrier_phase_l2_cycles[n];
                    d1 = doppler_l1[n];
                    fprintf(obs_file_ptr, "%#14.3f%-2s%#14.3f%-2s%#14.3f%-2s%#14.3f%-2s%#14.3f\n", c1, " ", p1, " ", p2, " ", l1, " ", d1);
                }
        }
    return false;
}

bool simple_rinex2_writer::write_binary_epoch(int nsats, const int *prn, gpstime *gps_time, const double *dist_m, const double *true_dist_m, const double *acc_carrier_phase_cycles, double *acc_carrier_phase_l2_cycles, const double *doppler_l1)
{
    double c = 299792458.0;
    double f_L1 = 1575.42e6;
    double f_L2 = 1227.60e6;

    // MULTIPLEXED FILE RECORDING - Record results to file, always 12 channels
    for (int n = 0; n < 12; n++)
        {
            if (d_dump == true)
                {
                    if (n < nsats)
                        {
                            try
                                {
                                    double tmp_double;
                                    tmp_double = gps_time->sec;
                                    d_dump_file.write((char *)&tmp_double, sizeof(double));
                                    tmp_double = doppler_l1[n];
                                    d_dump_file.write((char *)&tmp_double, sizeof(double));
                                    // CArrier phase as described in RINEX
                                    //tmp_double = (dist_m[n]/c)*f_L1;
                                    tmp_double = acc_carrier_phase_cycles[n];
                                    d_dump_file.write((char *)&tmp_double, sizeof(double));
                                    tmp_double = dist_m[n];
                                    d_dump_file.write((char *)&tmp_double, sizeof(double));
                                    tmp_double = true_dist_m[n];
                                    d_dump_file.write((char *)&tmp_double, sizeof(double));
                                    tmp_double = (dist_m[n] / c) * f_L1;
                                    d_dump_file.write((char *)&tmp_double, sizeof(double));
                                    tmp_double = (double)prn[n];
                                    d_dump_file.write((char *)&tmp_double, sizeof(double));
                                }
                            catch (const std::ifstream::failure &e)
                                {
                                    std::cout << "Exception writing observables dump file " << e.what() << std::endl;
                                }
                        }
                    else
                        {
                            try
                                {
                                    double tmp_double;
                                    tmp_double = 0.0;
                                    d_dump_file.write((char *)&tmp_double, sizeof(double));
                                    tmp_double = 0.0;
                                    d_dump_file.write((char *)&tmp_double, sizeof(double));
                                    tmp_double = 0.0;
                                    d_dump_file.write((char *)&tmp_double, sizeof(double));
                                    tmp_double = 0.0;
                                    d_dump_file.write((char *)&tmp_double, sizeof(double));
                                    tmp_double = 0.0;
                                    d_dump_file.write((char *)&tmp_double, sizeof(double));
                                    tmp_double = 0.0;
                                    d_dump_file.write((char *)&tmp_double, sizeof(double));
                                    tmp_double = 0.0;
                                    d_dump_file.write((char *)&tmp_double, sizeof(double));
                                }
                            catch (const std::ifstream::failure &e)
                                {
                                    std::cout << "Exception writing observables dump file " << e.what() << std::endl;
                                }
                        }
                }
        }
    return true;
}

bool simple_rinex2_writer::open_dump_file(std::string out_file)
{
    if (d_dump_file.is_open() == false)
        {
            try
                {
                    d_dump_filename = std::move(out_file);
                    d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
                    d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
                    //std::cout << "Observables dump enabled, Log file: " << d_dump_filename.c_str();
                    return true;
                }
            catch (const std::ifstream::failure &e)
                {
                    std::cout << "Problem opening Observables dump Log file: " << d_dump_filename.c_str();
                    return false;
                }
        }
    else
        {
            return false;
        }
}
bool simple_rinex2_writer::open_obs_file(const std::string &filename)
{
    // Open RINEX OBSERVATION output file
    if (nullptr == (obs_file_ptr = fopen(filename.c_str(), "wt")))
        {
            printf("Failed to open RINEX output file.\n");
            return false;
        }
    else
        {
            return true;
        }
}
